#define _RW_SP 0.0005
#define _RW_RHO 0.001


class PMCMC_SP
{
	public:
		PMCMC_SP(mat Y, mat X, mat W)
		{
			_p=X.n_cols;
			_n=X.n_rows;
			_W=W;
			_X=X;
			_Y=Y;
			mat m(_p,1);
			mat V(_p,_p);
			V.fill(_RW_SP);
			V=diagmat(V);
			m.fill(0);
			_In=eye(_n,_n);
			G=new Distribution::Gaussian(_p,m,V);
			boost::random::normal_distribution<> N2;
			_rnorm= new RandomG::Random<boost::random::normal_distribution<> >(N2);
			boost::random::uniform_01<> U;
			_runif=new RandomG::Random<boost::random::uniform_01<> >(U);
			R=new Resample::SysResample;
			_K=new KernelHMC(_p,V,m);
			_GHK=new Ghk<Resample::SysResample,KernelHMC>(R,_K);
			_above=new int[_n];
			_trunpt=new double[_n];
			for(int i=0;i<_n;i++)
			{
				_above[i]=1;
			}
			_eta=0.9;
			_s=20;
		}
		~PMCMC_SP()
		{
			delete G;
			delete _rnorm;
			delete _GHK;
			delete[] _trunpt;
			delete[] _above;
			delete _K;
		}

		vector<mat> operator()(int Mt, int Mx)
		{
			mat beta(Mt,_p);
			mat rho(Mt,1);
			rho(0,0)=(0.7*(*_runif)());
			beta.fill(0);
			double logv=logLikelihood(rho(0,0),beta(0,span::all),Mx)+lprior(beta(0,span::all));
			for(int i=1;i<Mt;i++)
			{
				mat add=(*G).r(1);
				mat betap=beta(i-1,span::all)+add.t();
				double rhop=rho(i-1,0)+_RW_RHO*(*_rnorm)();		
				cout << "beta" << beta(i-1,span::all) ;
				cout << "rho"<< rho(i-1,0);
				if(rhop>0.7 | rhop <0)
				{
					rho(i,0)=rho(i-1,0);
					beta(i,span::all)=beta(i-1,span::all);
				}else{
					double logvp=logLikelihood(rhop,betap,Mx)+lprior(betap);
					cout << "logv" << logvp-logv;
					double u=log((*_runif)());
					if(u<logvp-logv)
					{

						logv=logvp;
						rho(i,0)=rhop;
						beta(i,span::all)=betap;
					}else{
						rho(i,0)=rho(i-1,0);
						beta(i,span::all)=beta(i-1,span::all);
					}
				}
			}
			vector<mat> res;
			res.push_back(beta);
			res.push_back(rho);
			return res;

		}
		double logLikelihood(double rhop, mat betap, int Mx)
		{
					double res;
					mat Sig=inv(_In-rhop*_W);
					mat m=Sig*_X*betap.t();
					mat mL=chol(Sig);
					//mL=mL.t();
					double *L=new double[_n*_n];
					mat2vect<double>(mL,_n,_n,L);
					//cout << "tr";
					for(int j=0;j<_n;j++)
					{
						_trunpt[j]=m(j,0)*_Y(j,0);
					//	cout << _trunpt[j] << "\n";
					}
/*					for(int i=0;i<_n;i++)
						{
							for(int j=0;j<_n;j++)
							{
								cout << L[i*_n+j] << " ";
							}
							cout << "\n";
						}
*/
					(*_GHK).ghk_oneside(L,_trunpt,_above,&_n,&_eta,&Mx,&res,1);
					delete[] L;
			return res;
		}
	
		double lprior(mat theta)
		{
			double res=0;
			mat foo=theta;
			//cout << foo << "\n";
			//cout << "//" << bar;
			int d=theta.n_elem;
			//int d=_ds;
			mat ss(d,d);
			ss.fill(_s);
			ss=diagmat(ss);
			double  bar=as_scalar(foo*inv(ss)*foo.t());
			res=-0.5*d*log(2*PI)-0.5*log(det(ss))-0.5*bar;
			return res;

		}

	private:
		mat _X;
		mat _Y;
		mat _W;
		mat _In;
		Ghk<Resample::SysResample,KernelHMC> *_GHK;
		int _n;
		KernelHMC *_K;
		double _eta;
		int _p;
		double _s;
		int *_above;
		double *_trunpt;
		Distribution::Gaussian *G;
		RandomG::Random<boost::random::normal_distribution<> > *_rnorm;
		RandomG::Random<boost::random::uniform_01<> > *_runif;
		Resample::SysResample *R;
};
